/*
 * Math.cpp
 *
 *  Created on: Jan 21, 2014
 *      Author: ppks
 */

#include "common.h"
#include "MathMethods.h"

/******************************************************************************/
/* Local Variables Declaration                                                */
/******************************************************************************/


/******************************************************************************/
/* Local Functions Declaration                                                */
/******************************************************************************/


/******************************************************************************/
/* Function Code                                                              */
/******************************************************************************/

/*******************************************************************************
* Function Name: mul()
********************************************************************************
* Summary:
*  Performs optimized multiplication int16 x int16 in 22 cycles
*
* Parameters:
*  None
*
* Return:
*  None
*
*******************************************************************************/
int32_t  __attribute__ ((noinline)) mul(int16_t a, int16_t b)
{
    int32_t r;
    MultiS16X16to32(r, a, b);
    /* r = (int32_t)a*b; without asm requirement */
    return r;
}

/*******************************************************************************
* Function Name: _atan2()
********************************************************************************
* Summary:
*  Calculates ATAN2 and returns the value in degrees multiplied by 10.
*  Since the results are +/- pi/2 the output will be in the range +/- 900
*
* Parameters:
*  y - Input vector 1
*  x - Input vector 2
*
* Return:
*  atan2 value in degrees multiplied by 10
*
*******************************************************************************/
int16_t _atan2(int32_t y, int32_t x)
{
    float z = y;
    int16_t a;
    uint8_t c;
    c = abs(y) < abs(x);
    if ( c )
    {
        z = z / x;
    }
    else
    {
        z = x / z;
    }

    /* Calculate arctan (y/x) here */
    a = 2046.43 * (z / (3.5714 +  z * z));

    if ( c )
    {
        if (x < 0)
        {
            if (y < 0)
            {
                a -= 1800;
            }
            else
            {
                a += 1800;
            }
        }
    }
    else
    {
        a = 900 - a;
        if (y < 0)
        {
            a -= 1800;
        }
    }
    return a;
}

/*******************************************************************************
* Function Name: InvSqrt()
********************************************************************************
* Summary:
*  Calculates ATAN
*
* Parameters:
*  None
*
* Return:
*  None
*
*******************************************************************************/
float InvSqrt(float x)
{
    union{
        int32_t i;
        float   f;
    } conv;
    conv.f = x;
    conv.i = 0x5f3759df - (conv.i >> 1);
    return (0.5f * (conv.f * (3.0f - x * conv.f * conv.f)));
}
